#include <stdio.h>
#include <assert.h>
#include <math.h>
#include "aimage_utils.h"
#include "aimage.h"

void AIU::cls( AIMAGE &image, PIXVAL r, PIXVAL g, PIXVAL b )
{
	int count = 0;
	APIXEL p( r, g, b );
	
	AIMAGE::iterator cur, end;

	cur = image.begin();
	end = image.end();

	while ( cur != end )
	{
		*cur = p;
		++cur;
		count++;
	}
}

void AIU::blend( AIMAGE &dst, const AIMAGE &other, float percent )
{
	AIMAGE::iterator cur_dst, end_dst;
	AIMAGE::const_iterator cur_other, end_other;
	cur_dst = dst.begin();
	end_dst = dst.end();
	cur_other = other.const_begin();
	end_other = other.const_end();

	int second = percent * 32768;
	int first = 32768 - second;

	while ( cur_dst != end_dst )
	{
		APIXEL p, otherp;

		p = *cur_dst;
		otherp = *cur_other;

		int r,g,b;

		r = ((int ) p.r) * first + ((int) otherp.r ) * second;
		g = ((int ) p.g) * first + ((int) otherp.g ) * second;
		b = ((int ) p.b) * first + ((int) otherp.b ) * second;

		r/=32768;
		g/=32768;
		b/=32768;

		if ( r > FULL_INT ) r = FULL_INT;
		if ( g > FULL_INT ) g = FULL_INT;
		if ( b > FULL_INT ) b = FULL_INT;

		p.r = r;
		p.g = g;
		p.b = b;
		*cur_dst = p;
		++cur_dst;
		++cur_other;
	}

	assert( cur_other == end_other );

}

void AIU::gamma( AIMAGE &dst, float rg, float gg, float bg)
{
        int pre_gamma_r[ 65536 ];
        int pre_gamma_g[ 65536 ];
        int pre_gamma_b[ 65536 ];

        rg = 1.0 / rg;
        gg = 1.0 / gg;
        bg = 1.0 / bg;
        int i;

        for ( i = 0; i < 65536; ++i )
        {
                float f = logf( (( float) i ) / 65536.0f);
                float fg;

                fg = exp( f * rg);
                pre_gamma_r[ i ] = (int) (fg * 65536.0f);
                fg = exp( f * gg);
                pre_gamma_g[ i ] = (int) (fg * 65536.0f);
                fg = exp( f * bg);
                pre_gamma_b[ i ] = (int) (fg * 65536.0f);
        }

	AIMAGE::iterator cur_dst, end_dst;
	cur_dst = dst.begin();
	end_dst = dst.end();

	while ( cur_dst != end_dst )
	{
		APIXEL p, newp;

		p = *cur_dst;

		newp.r = pre_gamma_r[ p.r ];
		newp.g = pre_gamma_g[ p.g ];
		newp.b = pre_gamma_b[ p.b ];

		*cur_dst = newp;
		++cur_dst;
	}
}

void AIU::colrange( AIMAGE &image,
	float rmin, float rmax, 
	float gmin, float gmax, 
	float bmin, float bmax )
{
	unsigned int rlow, glow, blow;

	rlow = FULL_INT * rmin;
	glow = FULL_INT * gmin;
	blow = FULL_INT * bmin;

	AIMAGE::iterator cur, end;

	cur = image.begin();
	end = image.end();

	while ( cur != end )
	{
		APIXEL p;
		p = *cur;

		if ( p.r < rlow )
		{
			p.r = 0;
		}
		else
		{
			p.r -= rlow;
		}

		if ( p.g < glow )
		{
			p.g = 0;
		}
		else
		{
			p.g -= glow;
		}

		if ( p.b < blow )
		{
			p.b = 0;
		}
		else
		{
			p.b -= blow;
		}

		*cur = p;
		++cur;
	}
}

void AIU::stretch( AIMAGE &image,
	float mult, float f_cutoff )
{
	unsigned int cutoff;

	cutoff = FULL_INT * f_cutoff;

	AIMAGE::iterator cur, end;

	cur = image.begin();
	end = image.end();

	while ( cur != end )
	{
		APIXEL p;
		p = *cur;
		unsigned int r,g,b;

		if ( p.r < cutoff )
		{
			int r = p.r;
			r *= mult;
			if ( r > cutoff )
			{
				r = cutoff;
			}
			p.r = r;
		}

		if ( p.g < cutoff )
		{
			int g = p.g;
			g *= mult;
			if ( g > cutoff )
			{
				g = cutoff;
			}
			p.g = g;
		}

		if ( p.b < cutoff )
		{
			int b = p.b;
			b *= mult;
			if ( b > cutoff )
			{
				b = cutoff;
			}
			p.b = b;
		}

		*cur = p;
		++cur;
	}
}

float AIU::get_noise( const AIMAGE &image )
{
	long long noise = 0;
	long long count= 0;
	AIMAGE::const_iterator cur, end;

	cur = image.const_begin();
	end = image.const_end();

	while ( cur != end )
	{
		APIXEL p;

		p = *cur;

		noise += ((long long ) p.r ) * ((long long) p.r );
		noise += ((long long ) p.g ) * ((long long) p.g );
		noise += ((long long ) p.b ) * ((long long) p.b );

		count+=3;
		++cur;
	}

	float total = noise;
	total /= ((float) count );
	total = sqrtf( total );

	return total;
}

void AIU::colour_scale( AIMAGE &dst, float rs, float gs, float bs )
{
	AIMAGE::iterator dst_cur, dst_end;

	dst_cur = dst.begin();
	dst_end = dst.end();

	while ( dst_cur != dst_end )
	{
		APIXEL &p0 = *dst_cur;
		float r,g,b;

		r = ( float ) p0.r * rs;
		g = ( float ) p0.g * gs;
		b = ( float ) p0.b * bs;

		if ( r < FULL_INT )
		{
			p0.r = r;
		}
		else
		{
			p0.r = FULL_INT;
		}

		if ( g < FULL_INT )
		{
			p0.g = g;
		}
		else
		{
			p0.g = FULL_INT;
		}

		if ( b < FULL_INT )
		{
			p0.b = b;
		}
		else
		{
			p0.b = FULL_INT;
		}

		++dst_cur;
	}
}


void AIU::diff( AIMAGE &dst, const AIMAGE &src )
{
	AIMAGE::const_iterator src_cur, src_end;
	AIMAGE::iterator dst_cur, dst_end;

	src_cur = src.const_begin();
	src_end = src.const_end();
	dst_cur = dst.begin();
	dst_end = dst.end();

	while ( src_cur != src_end )
	{
		APIXEL p0, p1;
		int r0,g0,b0;
		int r1,g1,b1;

		p0 = *src_cur;
		p1 = *dst_cur;

		r0 = p0.r; g0 = p0.g; b0 = p0.b;
		r1 = p1.r; g1 = p1.g; b1 = p1.b;

		r0 -= r1;
		g0 -= g1;
		b0 -= b1;

		if ( r0 < 0 ) r0 = -r0;
		if ( g0 < 0 ) g0 = -g0;
		if ( b0 < 0 ) b0 = -b0;

		p0.r = r0;
		p0.g = g0;
		p0.b = b0;

		*dst_cur = p0;
		++dst_cur;
		++src_cur;
	}
}

void AIU::correct_for_flatframe( AIMAGE &dst, const AIMAGE &flat )
{
	AIMAGE::const_iterator flat_cur, flat_end;
	AIMAGE::iterator dst_cur, dst_end;

	flat_cur = flat.const_begin();
	flat_end = flat.const_end();
	dst_cur = dst.begin();
	dst_end = dst.end();

	while ( dst_cur != dst_end )
	{
		APIXEL p = *dst_cur;
		APIXEL flat = *flat_cur;

		unsigned int r,g, b, i;

		r = p.r;
		g = p.g;
		b = p.b;
		i = flat.r;

		r = r * FULL_INT;
		g = g * FULL_INT;
		b = b * FULL_INT;

		r /= i;
		g /= i;
		b /= i;

		if ( r > FULL_INT ) r = FULL_INT;
		if ( g > FULL_INT ) g = FULL_INT;
		if ( b > FULL_INT ) b = FULL_INT;

		p.r = r;
		p.g = g;
		p.b = b;
		*dst_cur = p;

		++dst_cur;
		++flat_cur;
	}
}

void AIU::correct_for_hotmap( AIMAGE &dst, const AIMAGE &hotmap)
{
	AIMAGE::const_iterator src_cur, src_end;
	AIMAGE::iterator dst_cur, dst_end;

	src_cur = hotmap.const_begin();
	src_end = hotmap.const_end();
	dst_cur = dst.begin();
	dst_end = dst.end();

	int x = 0;
	int y = 0;
	int width, height;

	dst.get_dim( width, height );

	while ( src_cur != src_end )
	{
		APIXEL hot = *src_cur;
		APIXEL dst = *dst_cur;

		if ( hot.r != 0 || hot.g != 0 || hot.b != 0 )
		{
			APIXEL sample_dst[ 8 ];
			APIXEL sample_hot[ 8 ];
			int num_samples = 0;

			if ( x > 0 )
			{
				sample_dst[ num_samples ] = *(dst_cur-1);
				sample_hot[ num_samples ] = *(src_cur-1);
				num_samples++;
				if ( y > 0 )
				{
					sample_dst[ num_samples ] = *(dst_cur-width-1);
					sample_hot[ num_samples ] = *(src_cur-width-1);
					num_samples++;
				}
				if ( y < height-1 )
				{
					sample_dst[ num_samples ] = *(dst_cur+width-1);
					sample_hot[ num_samples ] = *(src_cur+width-1);
					num_samples++;
				}
			}
			if ( x < width )
			{
				sample_dst[ num_samples ] = *(dst_cur+1);
				sample_hot[ num_samples ] = *(src_cur+1);
				num_samples++;
				if ( y > 0 )
				{
					sample_dst[ num_samples ] = *(dst_cur-width+1);
					sample_hot[ num_samples ] = *(src_cur-width+1);
					num_samples++;
				}
				if ( y < height-1 )
				{
					sample_dst[ num_samples ] = *(dst_cur+width+1);
					sample_hot[ num_samples ] = *(src_cur+width+1);
					num_samples++;
				}
			}
			if ( y > 0 )
			{
				sample_dst[ num_samples ] = *(dst_cur-width);
				sample_hot[ num_samples ] = *(src_cur-width);
				num_samples++;
			}
			if ( y < height-1 )
			{
				sample_dst[ num_samples ] = *(dst_cur+width);
				sample_hot[ num_samples ] = *(src_cur+width);
				num_samples++;
			}
			ACCUM_PIXEL ap;
			APIXEL ap_samples;

			ap.r = 0; ap.g = 0; ap.b = 0;
			ap_samples.r = 0; ap_samples.g = 0; ap_samples.b = 0;
 
			int iindex;

			for ( iindex = 0; iindex < num_samples; ++iindex )
			{
				if ( hot.r && sample_hot[ iindex ].r == 0 )
				{
					ap.r += sample_dst[ iindex ].r;
					ap_samples.r +=1;
				}
				if ( hot.g && sample_hot[ iindex ].g == 0 )
				{
					ap.g += sample_dst[ iindex ].g;
					ap_samples.g +=1;
				}
				if ( hot.b && sample_hot[ iindex ].b == 0 )
				{
					ap.b += sample_dst[ iindex ].b;
					ap_samples.b +=1;
				}
			} 
			if ( hot.r )
			{
				//assert( ap_samples.r > 0 );
				if ( ap_samples.r > 0 )
				{
					dst.r = ap.r / ap_samples.r;	
				}
				else
				{
					dst.r = 0;
				}		
			}
			if ( hot.g )
			{
				if ( ap_samples.g > 0 )
				{
					dst.g = ap.g / ap_samples.g;		
				}
				else
				{
					dst.g = 0;
				}	
			}
			if ( hot.b )
			{
				if ( ap_samples.b > 0 )
				{
					dst.b = ap.b / ap_samples.b;	
				}
				else
				{
					dst.b = 0;
				}		
			}
			*dst_cur = dst;
		}

		x+=1;
		if ( x == width )
		{
			x = 0;
			y+=1;
		}

		++dst_cur;
		++src_cur;
	}
}

void AIU::subtract( AIMAGE &dst, const AIMAGE &other )
{
	AIMAGE::iterator cur_dst, end_dst;
	AIMAGE::const_iterator cur_other, end_other;
	cur_dst = dst.begin();
	end_dst = dst.end();
	cur_other = other.const_begin();
	end_other = other.const_end();

	while ( cur_dst != end_dst )
	{
		APIXEL p, otherp;

		p = *cur_dst;
		otherp = *cur_other;

		int r,g,b;

		r = ((int ) p.r) - ((int) otherp.r );
		g = ((int ) p.g) - ((int) otherp.g );
		b = ((int ) p.b) - ((int) otherp.b );

		if ( r < 0 ) r = 0;
		if ( g < 0 ) g = 0;
		if ( b < 0 ) b = 0;

		p.r = r;
		p.g = g;
		p.b = b;
		*cur_dst = p;
		++cur_dst;
		++cur_other;
	}

	assert( cur_other == end_other );

}

void AIU::edge( AIMAGE &dst, const AIMAGE &other )
{
	int xdim, ydim;
	int xdstdim, ydstdim;
	APIXEL pzero( 0.0f, 0.0f, 0.0f );

	other.get_dim( xdim, ydim );
	dst.get_dim( xdstdim, ydstdim );

	assert( xdim == xdstdim );
	assert( ydim == ydstdim );

	for ( int x = 0; x < xdim; ++x )
	{
		int xl, xr;
		xl = x - 1;
		if ( xl < 0 ) xl = 0;
		xr = x + 1;
		if ( xr >= xdim ) xr = xdim-1;

		for ( int y = 0; y < ydim; ++y )
		{
			int yd, yu;
			yu = y - 1;
			if ( yu < 0 ) yu = 0;
			yd = y + 1;
			if ( yd >= ydim ) yd = ydim-1;

			APIXEL pc, pup, pdo, ple, pri;

			other.getpixel( x, y, pc );
			other.getpixel( x, yu, pup );
			other.getpixel( x, yd, pdo );
			other.getpixel( xl, y, ple );
			other.getpixel( xr, y, pri );

			APIXEL pdup, pddo, pdle, pdri;

			pdup.diff( pup, pc );
			pddo.diff( pdo, pc );
			pdle.diff( ple, pc );
			pdri.diff( pri, pc );

			APIXEL ptotal = pzero;

			ptotal.addto( pdup );
			ptotal.addto( pddo );
			ptotal.addto( pdle );
			ptotal.addto( pdri );

			dst.setpixel( x, y, ptotal );
		}
	}
}

void AIU::test_for_valid_pixel( 
	AIMAGE &dst, 
	int x, 
	int y, 
	int &r, 
	int &g, 
	int &b, 
	int &found_count 
)
{
	APIXEL p;
	int width, height;

	dst.get_dim( width, height );

	if ( x < 0 || y < 0 || x >= width || y >= height )
	{
		return;
	}

	dst.getpixel( x, y, p );

 	if ( p.r == 0 && p.g == FULL_INT && p.b == 0 )	return;
 	if ( p.r == 0 && p.g == 0 && p.b == FULL_INT )	return;

	r += p.r;
	g += p.g;
	b += p.b;
	found_count++;
}

void AIU::find_closest_pixel( AIMAGE &dst, int xc, int yc )
{
	int box;
	int width, height;
	int maxbox;

	maxbox = max( width, height );

	dst.get_dim( width, height );
	int found_count = 0;

	for ( box = 0; box < maxbox && found_count == 0; ++box )
	{
		int x0, y0, x1, y1;

		x0 = xc - box;
		x1 = xc + box;
		y0 = yc - box;
		y1 = yc + box;

		int r = 0, g = 0, b = 0;

		for ( int x = x0; x <= x1; ++x )
		{
			test_for_valid_pixel( dst, x, y0, r, g, b, found_count ); 		
			test_for_valid_pixel( dst, x, y1, r, g, b, found_count ); 		
		}
		for ( int y = y0+1; y <= y1-1; ++y )
		{
			test_for_valid_pixel( dst, x0, y, r, g, b, found_count ); 		
			test_for_valid_pixel( dst, x0, y, r, g, b, found_count ); 
		}
		if ( found_count != 0 )
		{
			r /= found_count;
			g /= found_count;
			b /= found_count;
			APIXEL p;
			p.r = r;
			p.g = g;
			p.b = b;
			dst.setpixel( xc, yc, p );
			break;
		}		
	}
}

void AIU::find_pixel( 
	AIMAGE &dst, 
	int xc, int yc, 
	int xd, int yd,
	APIXEL &p,
	int &distance
)
{
	distance = 0;
	int width, height;

	dst.get_dim( width, height );

	for (;;)
	{
		if ( xc < 0 || yc < 0 || xc >= width || yc >= height )
		{
			distance = -1;
			return;
		}
		dst.getpixel( xc, yc, p );

 		if ( !( p.r == 0 && p.g == FULL_INT && p.b == 0 ) &&
 			  !( p.r == 0 && p.g == 0 && p.b == FULL_INT ) )
		{
			return;
		}
		distance++;
		xc += xd; 
		yc += yd; 
	}
}

void AIU::fill_in_pixel_dir( 
	AIMAGE &dst, 
	int xc, int yc, 
	int xd, int yd,
	int &r, int &g, int &b, int &samples )
{
	APIXEL p0, p1;
	int distance0, distance1;
	
	find_pixel( dst, xc, yc, xd, yd, p0, distance0 );
	find_pixel( dst, xc, yc, -xd, -yd, p1, distance1 );

	if ( distance0 < 0 || distance1 < 0 )
	{
		return;
	}

	int total_distance = distance0 + distance1;

	float p0_percent = ((float) distance0) / ((float) total_distance ); 
	float p1_percent = ((float) distance1) / ((float) total_distance ); 
	
	r += p0.r * p0_percent + p1.r * p1_percent;
	g += p0.g * p0_percent + p1.g * p1_percent;
	b += p0.b * p0_percent + p1.b * p1_percent;
	samples++;
}

void AIU::fill_in_pixel_normal( AIMAGE &dst, int xc, int yc )
{
	int r = 0, g = 0, b = 0;
	int samples = 0;

	APIXEL p;
	dst.getpixel( xc, yc, p );

 	if ( !( p.r == 0 && p.g == FULL_INT && p.b == 0 ) &&
 		  !( p.r == 0 && p.g == 0 && p.b == FULL_INT ) )
	{
		return;
	}

	fill_in_pixel_dir( dst, xc, yc, 1, 0, r, g, b, samples );
	fill_in_pixel_dir( dst, xc, yc, 0, 1, r, g, b, samples );
	fill_in_pixel_dir( dst, xc, yc, 1, 1, r, g, b, samples );

	p.r = r / samples;
	p.g = g / samples;
	p.b = b / samples;

	dst.setpixel( xc, yc, p );
}

void AIU::blank_edges( AIMAGE &dst, const AIMAGE &edgemap, float thresholdf )
{
	AIMAGE::iterator cur_dst, end_dst;
	AIMAGE::const_iterator cur_other, end_other;
	cur_dst = dst.begin();
	end_dst = dst.end();
	cur_other = edgemap.const_begin();
	end_other = edgemap.const_end();
	int threshold = FULL_INT * thresholdf;

	int width, height;
	dst.get_dim( width, height );

	while ( cur_dst != end_dst )
	{
		APIXEL p, edgep;

		p = *cur_dst;
		edgep = *cur_other;

		int maxi;

		maxi = max( max ( edgep.r, edgep.g ), edgep.b );

		if ( maxi > threshold )
		{
			p.r = 0;
			p.g = FULL_INT;
			p.b = 0;
		}
		*cur_dst = p;
		++cur_dst;
		++cur_other;
	}

	assert( cur_other == end_other );
	find_closest_pixel( dst, 0, 0 );
	find_closest_pixel( dst, width-1, 0 );
	find_closest_pixel( dst, 0, height-1 );
	find_closest_pixel( dst, width-1, height-1 );

	for ( int x = 1; x < width-1; ++x )
	{
		fill_in_pixel_normal( dst, x, 0 );
		fill_in_pixel_normal( dst, x, height-1 );
	} 
	for ( int y = 1; y < height-1; ++y )
	{
		fill_in_pixel_normal( dst, 0, y );
		fill_in_pixel_normal( dst, width-1, y );
	} 
	for ( int x = 1; x < width -1; ++x )
	{
		for ( int y = 1; y < height-1; ++y )
		{
			fill_in_pixel_normal( dst, x, y );
		}
	}
}

void AIU::sample( AIMAGE &dst, const AIMAGE &src, float scale)
{
	int dst_width, dst_height;
	int src_width, src_height;

	float inv_scale = 1.0f / scale;

	dst.get_dim( dst_width, dst_height );
	src.get_dim( src_width, src_height );
	
	for ( int x = 0; x < dst_width; ++x )
	{
		for ( int y = 0; y < dst_height; ++y )
		{
			float xsrc, ysrc;

			xsrc = ((float) x )	* inv_scale - 0.5f;
			ysrc = ((float) y )	* inv_scale - 0.5f;

			int x0, y0, x1, y1;
			float dx, dy;

			x0 = (int) (xsrc);
			y0 = (int) (ysrc);
			x1 = x0 + 1;
			y1 = y0 + 1;

			dx = xsrc - ((float) x0 );
			dy = ysrc - ((float) y0 );

			if ( x0 < 0 ) x0 = 0;
			if ( y0 < 0 ) y0 = 0;
			if ( x1 < 0 ) x1 = 0;
			if ( y1 < 0 ) y1 = 0;
			if ( x0 >= src_width ) x0 = src_width-1; 
			if ( y0 >= src_height ) y0 = src_height-1; 
			if ( x1 >= src_width ) x1 = src_width-1; 
			if ( y1 >= src_height ) y1 = src_height-1; 

			APIXEL p00, p01, p10, p11;
			float m00, m01, m10, m11;

			m00 = (1.0f - dx) * (1.0f - dy);
			m01 = (1.0f - dx) * dy;
			m10 = dx * (1.0f - dy);
			m11 = dx * dy;

			src.getpixel( x0, y0, p00 );
			src.getpixel( x0, y1, p01 );
			src.getpixel( x1, y0, p10 );
			src.getpixel( x1, y1, p11 );

			APIXEL dp;

			dp.r = (int) (
					((float) p00.r) * m00 +
					((float) p01.r) * m01 +
					((float) p10.r) * m10 +
					((float) p11.r) * m11 );

			dp.g = (int) (
					((float) p00.g) * m00 +
					((float) p01.g) * m01 +
					((float) p10.g) * m10 +
					((float) p11.g) * m11 );

			dp.b = (int) (
					((float) p00.b) * m00 +
					((float) p01.b) * m01 +
					((float) p10.b) * m10 +
					((float) p11.b) * m11 );

			dst.setpixel( x, y, dp );
		}
	}
}

void AIU::make_flat_invadjust( AIMAGE &dst, const AIMAGE &other )
{
	AIMAGE::iterator cur_dst, end_dst;
	AIMAGE::const_iterator cur_other, end_other;
	cur_dst = dst.begin();
	end_dst = dst.end();
	cur_other = other.const_begin();
	end_other = other.const_end();

	int maxi = 0;

	while ( cur_dst != end_dst )
	{
		APIXEL p, otherp;

		otherp = *cur_other;

		int r,g,b;

		r = ((int) otherp.r );
		g = ((int) otherp.g );
		b = ((int) otherp.b );

		int i;
		i = ( r * 3 + g * 6 + b ) / 10;

		p.r = i;
		p.g = i;
		p.b = i;

		if ( i > maxi )
		{
			maxi = i;
		}

		*cur_dst = p;
		++cur_dst;
		++cur_other;
	}

	assert( cur_other == end_other );

	cur_dst = dst.begin();
	while ( cur_dst != end_dst )
	{
		APIXEL p;
		p = *cur_dst;

		unsigned int i;

		i = p.r;
		i = i * FULL_INT / maxi;
		p.r = i;
		p.g = i;
		p.b = i;
		*cur_dst = p;
		++cur_dst;
	}
}

